#include "imu_capture.h"


using namespace std;

#define IMU_UART					"/dev/ttyUSB1"
#define IMU_UART_BAUDRATE   		"115200"

#define ACC_UPDATE		0x01
#define GYRO_UPDATE		0x02
#define ANGLE_UPDATE	0x04
#define MAG_UPDATE		0x08
#define READ_UPDATE		0x80


void sleepMs(uint16_t timeMs)
{
	usleep(timeMs * 1000);
}


static long long getNanosecondsTimestamp()
{
    auto now = std::chrono::system_clock::now();
    auto duration = now.time_since_epoch();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
}



ImuCapture::ImuCapture():
	m_ttyFd(-1),
	m_cDataUpdate(0),
	m_cCmd(0xff),
	m_imuCaptureFlag(true),
	m_imuDataCallbackFunction(NULL)
{

}


ImuCapture::~ImuCapture()
{
	if(m_ttyFd > 0)
	{
		close(m_ttyFd);
		m_ttyFd = -1;
	}
}


static void SensorUartSend(uint8_t *data, uint32_t uiSize)
{
	ImuCaptureHandler.imuSerialWrite(data, uiSize);
}

static void SensorUartRec()
{
	ImuCaptureHandler.imuSerialRead();
}

static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
	ImuCaptureHandler.imuDataUpdata(uiReg, uiRegNum);
}




int ImuCapture::ttyConfigure(char *ttyPort, char *baudrate)
{
	int ttyFd;
	struct termios  oldTio = {0};
	struct termios newCfg = {0};
	int rate;

	assert(ttyPort != NULL && baudrate != NULL);

	ttyFd = open(ttyPort, O_RDWR | O_NOCTTY );

	if(ttyFd < 0 )
	{
		cout << "open " << ttyPort << " error" << endl;
		return -1;
	}

	if(tcgetattr(ttyFd, &oldTio) != 0)
	{
		cout << "tcgettattr failed" << endl;
		close(ttyFd);
		return -1;
	}

	if(strcmp(baudrate, "38400") == 0)
	{
		rate = B38400;
	}
	else if(strcmp(baudrate, "57600") == 0)
	{
		rate = B57600;
	}
	else if(strcmp(baudrate, "115200") == 0)
	{
		rate = B115200;
	}
	else if(strcmp(baudrate, "230400") == 0)
	{
		rate = B230400;
	}
	else
	{
		cout << "baud rate:" << baudrate << "is not support!" << endl;
		close(ttyFd);
		return -1;
	}

	//set the raw model
	cfmakeraw(&newCfg);
	newCfg.c_cflag |= CREAD;

	if(cfsetspeed(&newCfg, rate) < 0)
	{
		cout << "cfsetispeed error:" << baudrate << endl;
	}

	newCfg.c_cflag &= ~CSIZE;
	newCfg.c_cflag |= CS8;
	newCfg.c_cflag &= ~PARENB;
	newCfg.c_iflag &= ~INPCK;
	newCfg.c_cflag &= ~CSTOPB;
	newCfg.c_cc[VTIME] = 0;
	newCfg.c_cc[VMIN] = 0;

	if (tcflush(ttyFd, TCIOFLUSH) < 0)
	{
		cout << "tcflush error" << endl;
		return -1;
	}

	if(tcsetattr(ttyFd, TCSANOW, &newCfg) < 0)
	{
		cout << "tcsetattr failed" << endl;
		close(ttyFd);
		return -1;
	}

	return ttyFd;
}


bool ImuCapture::imuInit()
{
	//start a thread for receive imu data
	std::thread imuCaptureThread(SensorUartRec);

	WitSerialWriteRegister(SensorUartSend);
	WitRegisterCallBack(SensorDataUpdata);
	WitDelayMsRegister(sleepMs);

	bool ret = AutoScanSensor();

	if(!ret)
	{
		m_imuCaptureFlag = false;
		imuCaptureThread.join();
		return false;
	}


	float fAngleTmp = 0;

	while(1)
	{
		sleepMs(2);
		if(m_cDataUpdate)
		{
			for(int i = 0; i < 3; i++)
			{
				m_fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
				m_fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
			}

			if(sReg[TEMP] - sReg[Roll] > 10 | sReg[Roll] - sReg[TEMP] > 10)
			{
				m_fAngle[0] = sReg[Roll] / 32768.0f * 180.0f;
				fAngleTmp = m_fAngle[0];
			}
			else
			{
				m_fAngle[0] = fAngleTmp;
			}

			for(int i = 1; i<3;i++)
			{
				m_fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
			}

			if(m_cDataUpdate & ACC_UPDATE)
			{
				//printf("acc:%.3f %.3f %.3f\r\n", m_fAcc[0], m_fAcc[1], m_fAcc[2]);
				m_cDataUpdate &= ~ACC_UPDATE;

				long long currentTimestamp = getNanosecondsTimestamp();
				if(m_imuDataCallbackFunction != NULL)
				{
					(*m_imuDataCallbackFunction)(m_fAcc[0], m_fAcc[1], m_fAcc[2],
												m_fGyro[0], m_fGyro[1], m_fGyro[2], currentTimestamp);
				}
			}

			if(m_cDataUpdate & GYRO_UPDATE)
			{
				//printf("gyro:%.3f %.3f %.3f\r\n", m_fGyro[0], m_fGyro[1], m_fGyro[2]);
				m_cDataUpdate &= ~GYRO_UPDATE;
			}

			if(m_cDataUpdate & ANGLE_UPDATE)
			{
				//printf("angle:%.3f %.3f %.3f\r\n", m_fAngle[0], m_fAngle[1], m_fAngle[2]);
				m_cDataUpdate &= ~ANGLE_UPDATE;
			}

			if(m_cDataUpdate & MAG_UPDATE)
			{
				//printf("mag:%d %d %d\r\n", sReg[HX], sReg[HY], sReg[HZ]);
				m_cDataUpdate &= ~MAG_UPDATE;
			}
		}
	}

	m_imuCaptureFlag = false;
	imuCaptureThread.join();
	return true;
}


void ImuCapture::imuSerialWrite(uint8_t* data, uint32_t len)
{
	if(m_ttyFd < 0)
	{
		cout << "m_ttyFd < 0!" << endl;
		return;
	}


	int ret = write(m_ttyFd, data, len);
	if(ret < 0)
	{
		cout << "m_ttyFd write error!" << endl;
	}
}


void ImuCapture::imuSerialRead()
{
	uint8_t buf[512];
	int len;

	while(m_imuCaptureFlag)
	{
		sleepMs(2);

		if(m_ttyFd < 0)
		{
			continue;
		}

		do{
			len = 0;
			bzero(buf, sizeof(buf));
			len = read(m_ttyFd, buf, sizeof(buf));

			if(len < 0)
			{
				//cout << "m_ttyFd read error!" << endl;
			}

			for(int i=0; i<len; i++)
			{
				WitSerialDataIn(buf[i]);
			}

		}while(len == sizeof(buf));
	}
}

void ImuCapture::imuDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
	int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
//            case AX:
//            case AY:
            case AZ:
				m_cDataUpdate |= ACC_UPDATE;
            break;
//            case GX:
//            case GY:
            case GZ:
				m_cDataUpdate |= GYRO_UPDATE;
            break;
//            case HX:
//            case HY:
            case HZ:
				m_cDataUpdate |= MAG_UPDATE;
            break;
//            case Roll:
//            case Pitch:
            case Yaw:
				m_cDataUpdate |= ANGLE_UPDATE;
            break;
            default:
				m_cDataUpdate |= READ_UPDATE;
			break;
        }
		uiReg++;
    }
}


bool ImuCapture::AutoScanSensor()
{
	int i, iRetry;
	for(i = 1; i < 9; i++)
	{
		char baudrate[10];
		memset(baudrate, 0, sizeof(baudrate));
		sprintf(baudrate, "%d", m_uiBaud[i]);
		cout << "auto baudrate:" << baudrate << endl;
		m_ttyFd = ttyConfigure(IMU_UART, baudrate);

		iRetry = 2;
		do
		{
			m_cDataUpdate = 0;
			WitReadReg(AX, 3);
			sleepMs(100);
			if(m_cDataUpdate != 0)
			{
				cout << "current baudrate:" << baudrate << endl;
				return true;
			}

			iRetry--;
			close(m_ttyFd);
		}while(iRetry);
	}

	cout << "can not find sensor" << endl;
	return false;
}


bool ImuCapture::registerimuDataCallback(imuDataCallback function)
{
	if(m_imuDataCallbackFunction != NULL)
	{
		return false;
	}

	m_imuDataCallbackFunction = function;
	return true;

}


ImuCapture ImuCaptureHandler;
